#include "top_aimer/RosAimer.hpp"

namespace wmj {
TopAimer::TopAimer(std::string name) : rclcpp::Node(name)
{
  setParam(ROS_CFG);

  // 模型分为left,right,omni三个大类，每个大类给每个id都建1个top_aimer实例
  // 下标left:0-7, right:8-15, omni:16-23; 
  m_top_aimer_instance_num = 3 * m_detect_id_num;

  // 初始化Aimer实例
  for (int i = 0; i < m_top_aimer_instance_num; i++)
  {
    std::shared_ptr<wmj::Aimer> top_aimer;
    if (i < m_detect_id_num)
    {
      top_aimer = std::make_shared<wmj::Aimer>(LEFT_TOP_CFG);
    }
    else if (i < 2 * m_detect_id_num)
    {
      top_aimer = std::make_shared<wmj::Aimer>(RIGHT_TOP_CFG);
    }
    else
    {
      top_aimer = std::make_shared<wmj::Aimer>(OMNI_TOP_CFG);
    }
    if (i < 2 * m_detect_id_num)  //omni不需要roi
    {
      cv::Size2i size = cv::Size2i(m_input_width, m_input_height);
      top_aimer->setDeepROISize(size);
    }
    m_top_aimer.push_back(top_aimer);
    std::shared_ptr<KinematicModel> model = nullptr;
    m_model.push_back(model);
  }
  // 创建各类发布接受者
  rclcpp::CallbackGroup::SharedPtr callback_group_armors = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); //多线程接收"/Armors_base"
  auto sub_armors_opt = rclcpp::SubscriptionOptions();
  sub_armors_opt.callback_group = callback_group_armors;
  m_armors_sub = this->create_subscription<base_interfaces::msg::Armors>("/Armors_base", rclcpp::QoS(4), std::bind(&TopAimer::armors_cb, this, std::placeholders::_1), sub_armors_opt);

  m_status_pub = this->create_publisher<base_interfaces::msg::KinematicStatusOfAll>("/Top_status_of_all", 1);
  m_debug_observer = this->create_publisher<base_interfaces::msg::Armors>("debug_observer",10);
  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this -> get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}

void TopAimer::setParam(const std::string &file_path)
{
  cv::FileStorage fs(file_path, cv::FileStorage::READ);
  fs["debug"] >> m_debug;
  fs["detect_id_num"] >> m_detect_id_num;
  fs["ekf_on"] >> m_ekf_on;
  //映射id_1->0, id_2->1, id_3->2, id_4->3, id_5->4, id_7->5, id_11->6, id_8->7
  for (auto node: fs["mapping"])
  {
    int id, index;
    node["id"] >> id;
    node["index"] >> index;
    mapping[id] = index;
  }
  fs.release();
}

void TopAimer::armors_cb(base_interfaces::msg::Armors::SharedPtr msg)
{
  if (m_debug)
  {
    std::cout << "recv---------------------" << std::endl;
    std::cout << "[in]id: ";
    for (int i = 0; i < msg->num; i++)
    {
      std::cout << msg->armors[i].id << " ";
    }
    std::cout << std::endl;
  }

  // 创建wmj::Armors，将msg->armors分类存放至wmj::Armors
  std::vector<Armors> armors;
  std::vector<Armor> final_armors;
  for (int i = 0; i < m_top_aimer_instance_num; i++)
  {
    Armors a;
    armors.push_back(a);
  }

  // 判断msg->armors属于哪个大类，一次callback只对一个大类中的ekf模型进行更新
  DETECTEDTYPE type;
  if (msg->header.frame_id == "left_camera_frame")
  {
    type = DETECT_LEFT;
  } 
  else if (msg->header.frame_id == "right_camera_frame")
  {
    type = DETECT_RIGHT;
  }
  else if (msg->header.frame_id.find("omni") != std::string::npos)
  {
    type = DETECT_OMNI;
  }
  else
  {      
    std::cout << "error frame id: " << msg->header.frame_id << std::endl;
    return;
  }

  if (msg -> num == 0)
  {
    std::cout << "Armors Empty!" << std::endl;
  }
  else
  {
    if (m_debug)
    {
      std::cout << "recv " << msg->num << " armors" << std::endl;
    }
    
    for (auto &armor_msg : msg->armors) 
    {
      Armor armor;
      armor.m_time_seq = armor_msg.header.stamp.sec + armor_msg.header.stamp.nanosec * 1e-9;
      armor.m_id = armor_msg.id;
      armor.m_armor_type = ARMOR_SMALL;
      armor.m_color = _RED;
      armor.m_detectedtype = DETECT_LEFT;
      
      tf2::Quaternion tf_q;
      double roll, pitch, yaw;
      geometry_msgs::msg::Pose pose = armor_msg.armor_pose;
      
      pose = armor_msg.armor_pose;
      armor.m_position.x = pose.position.x;
      armor.m_position.y = pose.position.y;
      armor.m_position.z = pose.position.z;
      armor.m_yaw_angle = armor_msg.yaw_angle;     // 绝对系的yaw_angle TODO:yaw角度值待测,是否 -PI/2？

      if (m_debug)
      {
        std::cout << "m_time_seq: " << armor.m_time_seq << std::endl;
        std::cout << "m_id: " << armor.m_id << std::endl;
        std::cout << "m_armor_type: " << armor.m_armor_type << std::endl;
        std::cout << "m_color: " << armor.m_color << std::endl;
        std::cout << "m_detectedtype: " << armor.m_detectedtype << std::endl;
        std::cout << "m_position.x: " << armor.m_position.x << std::endl;
        std::cout << "m_position.y: " << armor.m_position.y << std::endl;
        std::cout << "m_position.z: " << armor.m_position.z << std::endl;
        std::cout << "m_yaw_angle: " << R2D(armor.m_yaw_angle) << std::endl;

      }

      final_armors.push_back(armor);

    //debug of observer 发布者

      // 分类存放
      if (mapping.count(armor.m_id))
      {
        armors[mapping[armor.m_id] + armor.m_detectedtype * m_detect_id_num].push_back(armor);  // 分类核心赋值语句！！！
      }
      else
      {
        std::cout << "error id" << std::endl;
        continue;
      }
    }
  }

    base_interfaces::msg::Armors observer_armors;
    for (auto armor : final_armors)
    {   
        /**
         * 在这里面，把所有的绝对系下装甲板信息发出去，方便 debug,这里有个问题，这里不会把所有装甲板发出去，只会发出去第一个
        */
        base_interfaces::msg::Armor observer_armor;
        observer_armor.id = armor.m_id;
        observer_armor.armor_pose.position.x = armor.m_position.x;
        observer_armor.armor_pose.position.y = armor.m_position.y;
        observer_armor.armor_pose.position.z = armor.m_position.z;
        observer_armor.yaw_angle = armor.m_yaw_angle;
        observer_armors.armors.push_back(observer_armor);
        
    }
    m_debug_observer->publish(observer_armors);

  base_interfaces::msg::KinematicStatusOfAll Kinematic_msg;
  Kinematic_msg.header.stamp = msg->header.stamp;

  // 所有模型都要发送
  for (int i = 0; i < m_top_aimer_instance_num; i++)
  {
    base_interfaces::msg::KinematicStatus status_msg;
    status_msg.enable = ((m_model[i] != nullptr) & m_ekf_on); //关闭m_ekf_on时，不发模型;
    status_msg.is_track = m_top_aimer[i]->isReady();
    status_msg.header.stamp = msg->header.stamp;

    m_model[i] = m_top_aimer[i]->getTargetStatus(armors[i]);

    if (!model2msg(m_model[i], status_msg, armors[i]))
    {
      status_msg.enable = false;
      status_msg.is_track = false;
    }

    Kinematic_msg.status.push_back(status_msg);
  }
  
  m_status_pub->publish(Kinematic_msg);
}

bool TopAimer::getTransform(std::string tar_frame, std::string src_frame, builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped &transform)
{
    // 使用lookupTransform()一定要用try-catch语句包围
    try {
      // 得到坐标src_frame->tar_frame的坐标变化
      transform = tf_buffer_->lookupTransform(tar_frame, src_frame, stamp, rclcpp::Duration::from_nanoseconds(50000));
    } 
    catch (const tf2::TransformException & ex) {
        RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            tar_frame.c_str(), src_frame.c_str(), ex.what());
        return false;
    }
    return true;
}

bool TopAimer::model2msg(std::shared_ptr<wmj::KinematicModel> status, base_interfaces::msg::KinematicStatus &status_msg, Armors armors)
{
  if (!armors.empty())
  {
    base_interfaces::msg::Armor armor;
    armor.armor_pose.position.x = armors[0].m_position.x;
    armor.armor_pose.position.y = armors[0].m_position.y;
    armor.armor_pose.position.z = armors[0].m_position.z;
    armor.yaw_angle = armors[0].m_yaw_angle;
    status_msg.armors.push_back(armor);
    status_msg.id = armors[0].m_id; // 不发送模型，就发送装甲板位姿和id
  }
  if(status == nullptr || !m_ekf_on)
  {
    return true;
  }
  else
  {
    status_msg.center.x = status -> center.x;
    status_msg.center.y = status -> center.y;
    status_msg.model_type = status -> model_type;
    
    switch (status -> model_type)
    {
      case KinematicModel::STANDARD:
        { 
          // 不同模型velocity变量类型不一样，用dynamic_pointer_cast父类转子类获取
          std::shared_ptr<StandardModel> standard_model = std::dynamic_pointer_cast<StandardModel>(status);
          status_msg.velocity.x = standard_model->getVelocity().x;
          status_msg.velocity.y = standard_model->getVelocity().y;
          break;
        }
      case KinematicModel::BALANCE:
        {
          std::shared_ptr<BalanceModel> balance_model = std::dynamic_pointer_cast<BalanceModel>(status);
          status_msg.velocity.x = balance_model->getVelocity();
          status_msg.velocity.y = 0;
          break;
        }
      case KinematicModel::OUTPOST:
        {
          status_msg.velocity.x = 0;
          status_msg.velocity.y = 0;
          break;
        }
      default:
        std::cout << "error model: " << status->model_type << std::endl;
        return false;
    }
    status_msg.height[0] = status -> height[0];
    status_msg.height[1] = status -> height[1];
    status_msg.height[2] = status -> height[2];
    status_msg.radius[0] = status -> radius[0];
    status_msg.radius[1] = status -> radius[1];
    status_msg.phase = status -> phase;
    status_msg.palstance = status -> palstance;
    status_msg.id = status -> id;
    
  }
  return true;
}

}  // namespace wmj

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<wmj::TopAimer>("top_aimer");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}